#include "read_boy_fb.h"
//#include "read_boy_1_54.h"
//#include "read_boy_base.h"
#include "read_boy_confg.h"
#include "read_boy.h"

#define FB_WIDTH_MAX 600
#define FB_HEIGHT_MAX 448

//#ifdef READ_BOY_DEV_5_83A
#define AAA
#ifdef  AAA
uint8_t read_boy_fb_for_A8[FB_HEIGHT_MAX][FB_WIDTH_MAX/8];

void read_boy_fb_set_one_pixel(int16_t iX,int16_t iY,uint8_t color){
    uint16_t x,x_mod;
    x = iX /8;
    x_mod = 7 - (iX %8);
    if(0 == color){
        read_boy_fb_for_A8[iY][x] = read_boy_fb_for_A8[iY][x] & (~(1<<x_mod));
    }else{
        read_boy_fb_for_A8[iY][x] = read_boy_fb_for_A8[iY][x] | (1<<x_mod);
    }
    
    //read_boy_fb_for_A8[iY][iX] = color;
}


uint8_t read_boy_fb_read_one_byte(uint32_t num){
    int x,y;
    //uint8_t dat = 0;
    y = num /(FB_WIDTH / 8);
    x = (num %(FB_WIDTH / 8))  ;
    return read_boy_fb_for_A8[y][x];
//    for(char i = 0; i < 8;i++){
//        if(read_boy_fb_for_A8[y][x + i]){
//            dat |= (1<<(7-i));
//        }

//    }
//    return dat;
}

//================================================


//void read_boy_fb_draw_bitmap(const unsigned char* image_buffer){
//    uint16_t x,y;
//    uint32_t Addr;
//    uint8_t dat;
//    for (y = 0; y < FB_HEIGHT; y++) {
//        for (x = 0; x < FB_WIDTH; x++) { 
//            Addr = (x + y * FB_WIDTH)/8;
//            dat = image_buffer[Addr];
//            if(dat & (1<<(7-(x%8)))){
//                read_boy_fb_for_A8[y][x] = 0xff;
//            }else{
//                read_boy_fb_for_A8[y][x]  = 0x0;
//            }
//        }
//        
//    }
//}


//extern const unsigned char gImage_1in54[5000];
void test_fb(){
    //read_boy_fb_draw_bitmap(gImage_1in54);
    read_boy_fb_clear();
}

void read_boy_fb_clear(){   
    for(int i = 0 ; i < (FB_HEIGHT); i++){
        for(int j = 0; j < FB_WIDTH/8; j++){
            read_boy_fb_for_A8[i][j] = 0xff;
        }
    }
}

void read_boy_fb_fillRect(int16_t x,int16_t y,int16_t w,int16_t h){
    uint16_t i,j;
    for(  j = 0; j < h; j++){
        for(  i = 0; i < w; i++){        
            //read_boy_fb_for_A8[ y+j ][x+i] = 0x00;
            
            read_boy_fb_set_one_pixel(x+i,y+j,0);
        }
    }
}

#else


uint8_t read_boy_fb_for_A8[FB_HEIGHT][FB_WIDTH];

void read_boy_fb_set_one_pixel(int16_t iX,int16_t iY,uint8_t color){
    
    read_boy_fb_for_A8[iY][iX] = color;
}


uint8_t read_boy_fb_read_one_byte(uint32_t num){
    int x,y;
    uint8_t dat = 0;
    y = num /(FB_WIDTH / 8);
    x = (num %(FB_WIDTH / 8)) *8;
    for(char i = 0; i < 8;i++){
        if(read_boy_fb_for_A8[y][x + i]){
            dat |= (1<<(7-i));
        }

    }
    return dat;
}

//================================================




//  1   1    1   0    1  0      0  0      0   0
//  1   1    1   1    0  1      1  0      0   0
//  204      153      102       51        0

void read_boy_Floyd_Steinberg_baier(unsigned char* image,const unsigned char* image_buffer,uint16_t width,uint16_t hight){
    uint16_t x,y,dat,index1,index2;    
    x = width / 2;
    y = hight / 2;
    for(int i =0; i < y; i++){
        index1 = i * 2 * width  ;
        index2 = (i * 2 + 1)* width  ;
        for(int j = 0; j < x; j++){
            dat = image_buffer[index1 + j*2] + image_buffer[index1 + j*2 + 1]
                    + image_buffer[index2 + j*2] + image_buffer[index2 + j*2 + 1];
            dat = dat >> 2;//dat  /4
            if(dat >= 204){
                image[index1 + j*2]     = 0xff;
                image[index1 + j*2 + 1] = 0xff;
                image[index2 + j*2]     = 0xff;
                image[index2 + j*2 + 1] = 0xff;
            }else if(dat >= 153){
                image[index1 + j*2]     = 0xff;
                image[index1 + j*2 + 1] = 0x00;
                image[index2 + j*2]     = 0xff;
                image[index2 + j*2 + 1] = 0xff;
            }else if(dat >= 102){
                image[index1 + j*2]     = 0xff;
                image[index1 + j*2 + 1] = 0x00;
                image[index2 + j*2]     = 0x00;
                image[index2 + j*2 + 1] = 0xff;
            }else if(dat >= 51){
                image[index1 + j*2]     = 0x00;
                image[index1 + j*2 + 1] = 0x00;
                image[index2 + j*2]     = 0xff;
                image[index2 + j*2 + 1] = 0x00;
            }else{
                image[index1 + j*2]     = 0x00;
                image[index1 + j*2 + 1] = 0x00;
                image[index2 + j*2]     = 0x00;
                image[index2 + j*2 + 1] = 0x00;
            }
        }
    
    }

}



//       x    7
//  3    5    1
//   1/16

void read_boy_Floyd_Steinberg_image( unsigned char* image,const unsigned char* image_buffer,uint16_t width,uint16_t hight){
    int16_t dat,err,index1,index2;
    int x,y;
    x = hight * width;
    for(  y = 0; y < x;y++){
        image[y] = image_buffer[y];
    }
    for(  y = 0; y < (hight - 1);y++){
        index1 = y * width;
        index2 = (y+1) * width;
        for(  x = 1; x < (width - 1); x++){
            if((image[ index1 + x]   )> 127){
                
                err = 0xff - image[ index1 + x]; 
                image[index1 + x] = 0xff;
                
                dat = (err * 7) >> 4;
                if(image[index1 + x + 1] > dat ){
                    image[index1 + x + 1] -= dat;
                }else{
                    image[index1 + x + 1] = 0;
                }
                
                
                dat = (err * 3) >> 4;
                if(image[index2 + x - 1] > dat ){
                    image[index2 + x - 1] -= dat;
                }else{
                    image[index2 + x - 1] = 0;
                }
                
                
                dat = (err * 5) >> 4;
                if(image[index2 + x - 0] > dat ){
                    image[index2 + x - 0] -= dat;
                }else{
                    image[index2 + x - 0] = 0;
                }
                
                dat = (err * 1) >> 4;
                if(image[index2 + x + 1] > dat ){
                    image[index2 + x + 1] -= dat;
                }else{
                    image[index2 + x + 1] = 0;
                }
                
            }else{
                err = image[ index1 + x];
                image[index1 + x] = 0x00;
                dat = ((err * 7) >> 4) + image[index1 + x + 1];
                image[index1 + x + 1] = dat;
                if(dat > 0xff){
                    image[index1 + x + 1] = 0xff;
                }
                
                
                dat = ((err * 3) >> 4) + image[index2 + x - 1];
                image[index2 + x - 1] = dat;
                if(dat > 0xff){
                    image[index2 + x - 1] = 0xff;
                }
                
                dat = ((err * 5) >> 4) + image[index2 + x  ];
                image[index2 + x  ] = dat;
                if(dat > 0xff){
                    image[index2 + x  ] = 0xff;
                }
                
                dat = ((err * 1) >> 4) + image[index2 + x + 1];
                image[index2 + x + 1] = dat;
                if(dat > 0xff){
                    image[index2 + x +1 ] = 0xff;
                }
                
            }
        
        }
    
    }
}



//extern const unsigned char gImage_1in54[5000];
void test_fb(){
    //read_boy_fb_draw_bitmap(gImage_1in54);
    read_boy_fb_clear();
}

void read_boy_fb_clear(){   
    for(int i = 0 ; i < (FB_HEIGHT); i++){
        for(int j = 0; j < FB_WIDTH; j++){
            //read_boy_fb_for_A8[i][j] = 0xff;
            read_boy_fb_set_one_pixel(j,i,0xff);
        }
    }
}

void read_boy_fb_fillRect(int16_t x,int16_t y,int16_t w,int16_t h){
    uint16_t i,j;
    for(  j = 0; j < h; j++){
        for(  i = 0; i < w; i++){        
            //read_boy_fb_for_A8[ y+j ][x+i] = 0x00;
            read_boy_fb_set_one_pixel(x+i,y+j,0x00);
        }
    }
}


#endif



void read_boy_fb_draw_A8_bitmap(uint16_t iX,uint16_t iY,uint16_t width,uint16_t hight,const unsigned char* image_buffer){
    uint16_t x,y;
    uint32_t Addr;
    uint8_t dat;
    for (y = 0; y < hight; y++) {
        for (x = 0; x < width; x++) { 
            Addr = (x + y * width);
            dat = image_buffer[Addr];
            if(dat >= 128){
                //read_boy_fb_for_A8[y][x] = 0x00;
                read_boy_fb_set_one_pixel(iX+x, iY+y,0xff);
            }else{
                read_boy_fb_set_one_pixel(iX+x, iY+y,0x00);
            }
        }
        
    }
}



//  1   1    1   0    1  0      0  0      0   0
//  1   1    1   1    0  1      1  0      0   0
//  204      153      102       51        0

void read_boy_Floyd_Steinberg_baier_to_fb(uint16_t iX,uint16_t iY,uint16_t width,uint16_t hight,const unsigned char* image_buffer){
    uint16_t x,y,dat;
    int32_t index1,index2;    
    x = width / 2;
    y = hight / 2;
    for(int i =0; i < y; i++){
        index1 = i * 2 * width  ;
        index2 = (i * 2 + 1)* width  ;
        for(int j = 0; j < x; j++){
            dat = image_buffer[index1 + j*2] + image_buffer[index1 + j*2 + 1]
                    + image_buffer[index2 + j*2] + image_buffer[index2 + j*2 + 1];
            dat = dat >> 2;//dat  /4
            if(dat >= 204){                
                read_boy_fb_set_one_pixel(iX+j*2,   iY+i*2,0xff);
                read_boy_fb_set_one_pixel(iX+j*2+1, iY+i*2,0xff);
                read_boy_fb_set_one_pixel(iX+j*2,   iY+i*2+1,0xff);
                read_boy_fb_set_one_pixel(iX+j*2+1, iY+i*2+1,0xff);
            }else if(dat >= 153){
                read_boy_fb_set_one_pixel(iX+j*2,   iY+i*2,0xff);
                read_boy_fb_set_one_pixel(iX+j*2+1, iY+i*2,0x00);
                read_boy_fb_set_one_pixel(iX+j*2,   iY+i*2+1,0xff);
                read_boy_fb_set_one_pixel(iX+j*2+1, iY+i*2+1,0xff);
            }else if(dat >= 102){
                read_boy_fb_set_one_pixel(iX+j*2,   iY+i*2,  0xff);
                read_boy_fb_set_one_pixel(iX+j*2+1, iY+i*2,  0x00);
                read_boy_fb_set_one_pixel(iX+j*2,   iY+i*2+1,0x00);
                read_boy_fb_set_one_pixel(iX+j*2+1, iY+i*2+1,0xff);
            }else if(dat >= 51){
                read_boy_fb_set_one_pixel(iX+j*2,   iY+i*2,  0x00);
                read_boy_fb_set_one_pixel(iX+j*2+1, iY+i*2,  0x00);
                read_boy_fb_set_one_pixel(iX+j*2,   iY+i*2+1,0xff);
                read_boy_fb_set_one_pixel(iX+j*2+1, iY+i*2+1,0x00);
            }else{
                read_boy_fb_set_one_pixel(iX+j*2,   iY+i*2,  0x00);
                read_boy_fb_set_one_pixel(iX+j*2+1, iY+i*2,  0x00);
                read_boy_fb_set_one_pixel(iX+j*2,   iY+i*2+1,0x00);
                read_boy_fb_set_one_pixel(iX+j*2+1, iY+i*2+1,0x00);
            }
        }
    
    }

}

//       x    7
//  3    5    1
//   1/16
void read_boy_Floyd_Steinberg_to_fb(uint16_t iX,uint16_t iY,uint16_t width,uint16_t hight, int16_t* image2,const unsigned char* image_buffer ){    
    int32_t index;
    int16_t err;
    int16_t x,y;
    char num1 = 0;
    char num2 = 1;
    int16_t errIndex1,errIndex2;       
    for(  y = 0; y < width; y++){
        image2[y] = 0;
    }
    for(  y = 0; y < (hight  );y++){
        index = y * width;
        errIndex1 = num1 * width;
        errIndex2 = num2 * width;
        for(  x = 0; x < (width ); x++){
            if((image_buffer[ index + x] + image2[errIndex1 + x ] )> 127){                
                err =   image_buffer[ index + x]    + image2[errIndex1 + x ] - 0xff; 
                read_boy_fb_set_one_pixel(iX+x, iY+y,  0xff);                               
            }else{
                err = image_buffer[ index + x] + image2[errIndex1 + x ];
                read_boy_fb_set_one_pixel(iX+x, iY+y,  0x00);               
            }
            image2[errIndex1 + x +1] +=  (err * 7) / 16;
            if(x){
                image2[errIndex2 + x -1]  = (err * 3) / 16;
            }
            image2[errIndex2 + x   ]  = (err * 5) / 16;
            image2[errIndex2 + x +1]  = (err * 1) / 16;        
        }
        num1 = num2;//(num1+1) & 0x01;
        num2 = (num2+1) & 0x01;        
    }
}

//常用的灰度转换公式为：Gray = 0.299 * R + 0.587 * G + 0.114 * B
//Gray = 0.3 * R + 0.6 * G + 0.1  * B

uint8_t read_boy_rgb565_to_grey(uint16_t pixel) {
    uint32_t r, g, b;
    uint8_t Gray;
    r = (pixel >> 11) & 0x1F;
    g = (pixel >> 5) & 0x3F;
    b = pixel & 0x1F;

    // 转换为8位值
    r = (r << 3);
    g = (g << 2);
    b = (b << 3);

    // 计算灰度值，使用四舍五入
    Gray = (uint8_t)(((3 * r + 6 * g + b) + 5) / 10);

    return Gray;
}

uint8_t read_boy_rgb565_to_grey4(uint16_t pixel){
    uint32_t r,g,b;
    uint8_t Gray;
    r =  (pixel >> 11) & 0x1F;
    g = (pixel >> 5) & 0x3F;
    b = pixel & 0x1F;
    
    // 转换为8位值
    r = (r * 255) / 10;
    g = (g * 255) / 10;
    b = (b * 255) / 31;
    
    Gray = ( (r  + g + b) / 10) ;
    return Gray;
}

uint8_t read_boy_rgb565_to_grey3(uint16_t pixel){
    uint32_t r,g,b;
    uint8_t Gray;
    r =  (pixel >> 11) & 0x1F;
    g = (pixel >> 5) & 0x3F;
    b = pixel & 0x1F;
    
    // 转换为8位值
    r = (r * 255) / 31;
    g = (g * 255) / 63;
    b = (b * 255) / 31;
    
    Gray = ( (3*r  + 6*g + b) / 10) ;
    return Gray;
}

uint8_t read_boy_rgb565_to_grey2(uint16_t pixel) {
    uint32_t r, g, b;
    uint8_t Gray;
    r = (pixel >> 11) & 0x1F;
    g = (pixel >> 5) & 0x3F;
    b = pixel & 0x1F;

    // 转换为8位值
    r = (r * 255) / 31;
    g = (g * 255) / 63;
    b = (b * 255) / 31;

    // 计算灰度值，使用标准公式并进行四舍五入
    Gray = (uint8_t)((0.299 * r + 0.587 * g + 0.114 * b) + 0.5);

    return Gray;
}

void read_boy_fb_draw_rgb565_bitmap(uint16_t iX,uint16_t iY,uint16_t width,uint16_t hight,const uint16_t* image_buf ){
    uint16_t x,y;
    uint32_t Addr;
    uint8_t dat;
    //uint8_t gray;
    for (y = 0; y < hight; y++) {
        for (x = 0; x < width; x++) { 
            Addr = (x + y * width);
            dat = read_boy_rgb565_to_grey(image_buf[Addr]);
            if(dat >= 128){
                //read_boy_fb_for_A8[y][x] = 0x00;
                read_boy_fb_set_one_pixel(iX+x, iY+y,0xff);
            }else{
                read_boy_fb_set_one_pixel(iX+x, iY+y,0x00);
            }
        }
        
    }
}

void read_boy_Floyd_Steinberg_to_fb_rgb565(uint16_t iX,uint16_t iY,uint16_t width,uint16_t hight, int16_t* image2,const uint16_t* image_buf ){    
    int32_t index;
    int16_t err;
    int16_t x,y;
    uint8_t gray;
    char num1 = 0;
    char num2 = 1;
    int16_t errIndex1,errIndex2;       
    for(  y = 0; y < width; y++){
        image2[y] = 0;
    }
    for(  y = 0; y < (hight  );y++){
        index = y * width;
        errIndex1 = num1 * width;
        errIndex2 = num2 * width;
        for(  x = 0; x < (width ); x++){
            gray = read_boy_rgb565_to_grey(image_buf[ index + x]);
            if((gray + image2[errIndex1 + x ] )> 127){                
                err =   gray    + image2[errIndex1 + x ] - 0xff; 
                read_boy_fb_set_one_pixel(iX+x, iY+y,  0xff);                               
            }else{
                err = gray + image2[errIndex1 + x ];
                read_boy_fb_set_one_pixel(iX+x, iY+y,  0x00);               
            }
            image2[errIndex1 + x +1] +=  (err * 7) / 16;
            if(x){
                image2[errIndex2 + x -1]  = (err * 3) / 16;
            }
            image2[errIndex2 + x   ]  = (err * 5) / 16;
            image2[errIndex2 + x +1]  = (err * 1) / 16;        
        }
        num1 = num2;//(num1+1) & 0x01;
        num2 = (num2+1) & 0x01;        
    }
}

void read_boy_Floyd_Steinberg_baier_to_fb_rgb565(uint16_t iX,uint16_t iY,uint16_t width,uint16_t hight,const uint16_t* image_buf){
    uint16_t x,y,dat;
    int32_t index1,index2;  
    uint8_t gray1,gray2,gray3,gray4;    
    x = width / 2;
    y = hight / 2;
    for(int i =0; i < y; i++){
        index1 = i * 2 * width  ;
        index2 = (i * 2 + 1)* width  ;
        for(int j = 0; j < x; j++){
            gray1 = read_boy_rgb565_to_grey(image_buf[index1 + j*2]);
            gray2 = read_boy_rgb565_to_grey(image_buf[index1 + j*2+1]);
            gray3 = read_boy_rgb565_to_grey(image_buf[index2 + j*2]);
            gray4 = read_boy_rgb565_to_grey(image_buf[index2 + j*2+1]);
            
//            dat = image_buffer[index1 + j*2] + image_buffer[index1 + j*2 + 1]
//                    + image_buffer[index2 + j*2] + image_buffer[index2 + j*2 + 1];
            dat = gray1 + gray2 +gray3 + gray4;
            dat = dat >> 2;//dat  /4
            if(dat >= 204){                
                read_boy_fb_set_one_pixel(iX+j*2,   iY+i*2,0xff);
                read_boy_fb_set_one_pixel(iX+j*2+1, iY+i*2,0xff);
                read_boy_fb_set_one_pixel(iX+j*2,   iY+i*2+1,0xff);
                read_boy_fb_set_one_pixel(iX+j*2+1, iY+i*2+1,0xff);
            }else if(dat >= 153){
                read_boy_fb_set_one_pixel(iX+j*2,   iY+i*2,0xff);
                read_boy_fb_set_one_pixel(iX+j*2+1, iY+i*2,0x00);
                read_boy_fb_set_one_pixel(iX+j*2,   iY+i*2+1,0xff);
                read_boy_fb_set_one_pixel(iX+j*2+1, iY+i*2+1,0xff);
            }else if(dat >= 102){
                read_boy_fb_set_one_pixel(iX+j*2,   iY+i*2,  0xff);
                read_boy_fb_set_one_pixel(iX+j*2+1, iY+i*2,  0x00);
                read_boy_fb_set_one_pixel(iX+j*2,   iY+i*2+1,0x00);
                read_boy_fb_set_one_pixel(iX+j*2+1, iY+i*2+1,0xff);
            }else if(dat >= 51){
                read_boy_fb_set_one_pixel(iX+j*2,   iY+i*2,  0x00);
                read_boy_fb_set_one_pixel(iX+j*2+1, iY+i*2,  0x00);
                read_boy_fb_set_one_pixel(iX+j*2,   iY+i*2+1,0xff);
                read_boy_fb_set_one_pixel(iX+j*2+1, iY+i*2+1,0x00);
            }else{
                read_boy_fb_set_one_pixel(iX+j*2,   iY+i*2,  0x00);
                read_boy_fb_set_one_pixel(iX+j*2+1, iY+i*2,  0x00);
                read_boy_fb_set_one_pixel(iX+j*2,   iY+i*2+1,0x00);
                read_boy_fb_set_one_pixel(iX+j*2+1, iY+i*2+1,0x00);
            }
        }
    
    }

}

//void read_boy_Floyd_Steinberg_to_fb(uint16_t iX,uint16_t iY,uint16_t width,uint16_t hight, int16_t* image2,const unsigned char* image_buffer ){
//    int16_t dat,err,index1,index2,next;
//    int x,y;
//    //x =  * width;
//    char num1 = 0;
//    char num2 = 1;
//    next = 0;
//    int16_t errIndex1,errIndex2;
//    
//    
//    for(  y = 0; y < width;y++){
//        image2[y+width] = 0;
//    }
//    for(  y = 0; y < (hight  );y++){
//        index1 = y * width;
//        index2 = (y+1) * width;
//        errIndex1 = num2 * width;
//        errIndex2 = num1 * width;
//        for(  x = 0; x < (width ); x++){
//            if((image_buffer[ index1 + x]  + next + image2[errIndex1 + x ] )> 127){
//                
//                err =   image_buffer[ index1 + x]  + next + image2[errIndex1 + x ] - 0xff; 
//                read_boy_fb_set_one_pixel(iX+x,   iY+y,  0xff);
//                 
//               
//            }else{
//                err = image_buffer[ index1 + x]  + next + image2[errIndex1 + x ];
//                read_boy_fb_set_one_pixel(iX+x,   iY+y,  0x00);
//               
//            }
//            next =  (err * 7) / 16;
//            if(x)
//            image2[errIndex2 + x -1]  = (err * 3) / 16;
//            image2[errIndex2 + x   ]  = (err * 5) / 16;
//            image2[errIndex2 + x +1]  = (err * 1) / 16;
//        
//        }
//        num1++;
//        num1 = num1 % 2;
//        num2++;
//        num2 = num2 % 2;
//    
//    }
//}







